import CommandClass as CC
import Project.Utilities.Utilities as Utilities
import Project.Simulation.SimulationInterface as SimulationInterface
import Project.Main as Main
import Project.Tracker.TrackerInterface as TrackerInterface
from math import *

import copy

# Initializes the List Fields and First Commands
class CommandGeneratorClass:
    def __init__(self, path, numOfCommandsTillFix, robotArray, sourcePoints):
        
        self.path = path
        self.numOfCommandsTillFix = numOfCommandsTillFix
        
        self.items = []
        self.readLinesFromPath(robotArray, sourcePoints)
        self.item_length = len(self.items)
        self.position = -1
    
    # Return an object which has a next() method. In this instance, we can just return self.
    def __iter__(self):
        return self
   
    # Retrieve Next Command (if has any)
    def next(self):
        self.position += 1
        if self.position >= self.item_length:
            raise StopIteration
        return self.items[self.position]
    
    def hasNext(self):
        if self.position + 1 >= self.item_length:
            return 0
        return 1
    
    def getPos(self):
        return self.position
    
    def getNumCommandTillFix(self):
        return self.numOfCommandsTillFix
    
    def close(self):
        command = self.items[1]
        command.close()
            
    

        
    
    # Add Commands 
    # TODO: Consider dividing commands (by time \ distance)
    def readLinesFromPath(self, robotArray, lastPoints):
            counter = 0
            Angle = 0
            for i in range (0 ,len(lastPoints)):
                lastPoints[i] = Utilities.normalizationBack(lastPoints[i], Main.WORKSPACE_SIZE)
           
            for line in open(self.path).read().split('\n'):
                lineParts = line.split(" ")
                if (lineParts[0] == "id"):
                    currID = int(lineParts[1]) - 1
                    

                elif (lineParts[0] == "angle"):
                    Angle = float(lineParts[1])
                    Distance = 0
                    currPoint = lastPoints[currID]
                    newAngle = Angle # Positive or Negative Direction 
                    numOfCommands = len(self.items)
                    
                    if (numOfCommands < 1):
                        currCommand = CC.CommandClass(currID, Distance,newAngle , currPoint, currPoint) #mirror angle
                        
                    else :
                        prevCommand = self.items[numOfCommands - 1]
                        if (abs(prevCommand.Distance) == 0 and (prevCommand.Angle != 0) and (prevCommand.getID() == (currID - Main.NUM_OF_ROBOTS))): #Signifying two angles in a row
                            prevCommand.Angle += newAngle
                            if (prevCommand.Angle < 0):
                                prevCommand.Angle %= -360
                            else:
                                prevCommand.Angle %= 360
                                self.items[numOfCommands - 1] =    prevCommand
                            currCommand = prevCommand
                        else:
                            currCommand = CC.CommandClass(currID, Distance,newAngle , currPoint, currPoint) #mirror angle
                            
                    currCommand.displayCommand()
                    self.items.append(currCommand);
                    counter += 1
                    robotArray[currID].azimuth += newAngle
                    if (robotArray[currID].azimuth < 0):
                        robotArray[currID].azimuth %= -360
                    else:
                        robotArray[currID].azimuth %= 360
               
                elif (lineParts[0] == "line"):
                    p1 = [float(lineParts[1]),float(lineParts[2])]
                    p1 = Utilities.normalizationBack(p1, Main.WORKSPACE_SIZE)
                    print(currID)
                    
                    pointTo = TrackerInterface.PointClass.Point()
                    pointTo.setX(p1[0])
                    pointTo.setY(p1[1])
                    
                    pointFrom = TrackerInterface.PointClass.Point()
                    pointFrom.setX(lastPoints[currID][0])
                    pointFrom.setY(lastPoints[currID][1])
                    
                    
                    currAngle = robotArray[currID].azimuth
                    
                    '''if (abs(NewAngle - currAngle) <= 180 + Main.ANGLE_DIRECTION_EPSILON) and (abs(NewAngle - currAngle) > 180 - Main.ANGLE_DIRECTION_EPSILON):
                        pre = -1
                    else:
                        pre = 1
                    '''
                    
                    # Check if we need to go back or forward  
                    
                    # normalize target point              
                    pointTo = Utilities.pointNormalization(pointTo)     
                    pointFrom = Utilities.pointNormalization(pointFrom)
                    
                    # create target vector
                    targetVector = TrackerInterface.PointClass.Point()
                    targetVector.setX(pointTo.getX() - pointFrom.getX())
                    targetVector.setY(pointTo.getY() - pointFrom.getY())
                    
                    if (abs(targetVector.getX()) < 0.00001):
                         targetVector.setX(0.0)
                    if (abs(targetVector.getY()) < 0.00001):
                         targetVector.setY(0.0)
                    
                    # create vertical vector
                    verticalVector = TrackerInterface.PointClass.Point()
                    verticalVector.setX(cos((currAngle + 90)*pi/180))
                    verticalVector.setY(sin((currAngle + 90)*pi/180))
                    
                    print("*************************************************************")
                    print("Point from = (" + str(pointFrom.x) + ", " + str(pointFrom.y) + "), angle: " + str(currAngle))
                    print("Point to = (" + str(pointTo.x) + ", " + str(pointTo.y) + ")")
                    print("Target vector is: (" + str(targetVector.x) + ", " + str(targetVector.y) + ")")
                    print("Vertical vector is: (" + str(verticalVector.x) + ", " + str(verticalVector.y) + ")")
                    print( "robot " + str(currID + Main.OFFSET_ROBOTS) + ", azimuth is: " +  str(robotArray[currID].azimuth))
                    print("*************************************************************")
                    


                    #calculate position      
                    position = ((verticalVector.getX() * targetVector.getY()) - (verticalVector.getY() * targetVector.getX()))
                    print("Position = " + str(position))
                    
                    if (position < 0):
                        pre = 1
                    else:
                        pre = -1
                    
                    
                        
                    Distance = Utilities.distance(lastPoints[currID],p1) * pre
                    
                    currPoint = lastPoints[currID]
                    newPoint = p1
                    lastPoints[currID] = p1
                    Angle = 0          
                    currCommand = CC.CommandClass(currID, Distance, Angle, currPoint, newPoint)
                    currCommand.displayCommand()
                    self.items.append(currCommand);
                    counter += 1